/*
 * Helper task to manage IR sensors
 */

//working version

#include "xtask.h"
#include "sensor_read.h"
#include "ir_manager.h"

using namespace kthrobot;
using namespace std;

namespace Group5 {

    double IRManager::EstimateAngleToRightWall() {
        if(!FrontRightFree() && !RearRightFree()) {
            return estimate_angle_(FrontRightRange(), RearRightRange());
        }
        else {
            return 0;
        }
    }


    double IRManager::EstimateAngleToLeftWall() {
        if(!FrontLeftFree() && !RearLeftFree()) {
            return estimate_angle_(FrontLeftRange(), RearLeftRange());
        }
        else {
            return 0;
        }
    }


    double IRManager::estimate_angle_(int front_range, int rear_range) {
        // Estimates the angle to the wall from the rear and front distances
        int dist = front_range - rear_range;
        if (dist < 17 && dist >= 10) {
            return 45;
        }
        else if (dist >= 5){
            return 30;
        }
        else if (dist >= 3){
            return 15;
        }
        else if (dist > -1){
            return 0;
        }
        else if (dist > -4){
            return -20;
        }
        else if (dist > -8){
            return -40;
        }
        else if (dist > -17){
            return -60;
        }
        else return 0;
    }
};
